﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace MapViewer
{
   public class Search
    {

       static Dictionary<Node, Node> came_from = new Dictionary<Node, Node>();
       static Dictionary<Node, int> indexes = new Dictionary<Node, int>();

     public static List<Node> Astar(Node start, Node goal, List<Node> allNodes)
    {
        came_from.Clear();
        indexes.Clear();

        for (int i = 0; i < allNodes.Count; i++)
            indexes.Add(allNodes[i], i);

        List<Node> closedset = new List<Node>();    // The set of nodes already evaluated.
        List<Node> openset = new List<Node>(); openset.Add(start);    // The set of tentative nodes to be evaluated, initially containing the start node

        float[] g_score = new float[allNodes.Count];
         float[] h_score = new float[allNodes.Count];
         float[] f_score = new float[allNodes.Count];

        g_score[indexes[start]] = 0;   // Cost from start along best known path.
        h_score[indexes[start]] = heuristic_cost_estimate(start, goal);
        f_score[indexes[start]] = g_score[indexes[start]] + h_score[indexes[start]];    // Estimated total cost from start to goal through y.
 
        while(openset.Count > 0)
        {
            Node current = openset[0];//:= the node in openset having the lowest f_score[] value
            for(int i = 1; i < openset.Count; i++)
                if(f_score[indexes[openset[i]]] < f_score[indexes[current]]) current = openset[i];


            if (current == goal)
                return reconstruct_path(came_from[goal]);
 
            //remove current from openset
            openset.Remove(current);

            //add current to closedset
            closedset.Add(current);

            foreach(Node neighbor in current.connected)
            {
                if(closedset.Contains(neighbor))
                    continue;
                float tentative_g_score = g_score[indexes[current]] + dist_between(current, neighbor);
 
                bool tentative_is_better;
                if(!openset.Contains(neighbor))
                {
                    //add neighbor to openset
                    openset.Add(neighbor);

                    h_score[indexes[neighbor]] = heuristic_cost_estimate(neighbor, goal);
                    tentative_is_better = true;
                }
                else if(tentative_g_score < g_score[indexes[neighbor]])
                    tentative_is_better = true;
                else
                    tentative_is_better = false;
 
                if(tentative_is_better)
                {
                    came_from[neighbor] = current;
                    g_score[indexes[neighbor]] = tentative_g_score;
                    f_score[indexes[neighbor]] = g_score[indexes[neighbor]] + h_score[indexes[neighbor]];
                }
            }
        }
        return null;//failure
     }
 
        public static List<Node> reconstruct_path(Node current_node)
        {
            List<Node> retn = new List<Node>();
            if (came_from.Keys.Contains(current_node))
            {
                List<Node> p = reconstruct_path(came_from[current_node]);
                retn.AddRange(p);
                retn.Add(current_node);
                return retn;
            }
            else
            {
                retn.Add(current_node);
                return retn;
            }
        }

        public static float heuristic_cost_estimate(Node n1, Node n2)
        {
            return 1f;
        }

        public static float dist_between(Node n1, Node n2)
        {
            return 1f;
        }
    }
}
